//
// Created by 86156 on 25-7-4.
//

#include "motor.h"
#include "cmsis_os.h"
#include "control_driver.h"
#include "gyro_driver.h"
#include "pid_driver.h"
#include "motor_driver.h"
#include "rc_driver.h"


uint8_t JY60_Mode[] = {0xFF,0xAA,0x60};
uint8_t JY60_Calibrate[] = {0xFF, 0xAA, 0x67};
void Mode_Init()
{

    // CCR_mode.Set_flag 		= 0;
    TEST_mode.Enable_flag	    = 0;
    // RC_mode.Enable_flag	    = 0;

    TEST_mode.Target_Yaw		  = 0;
    TEST_mode.Target_Depth       = 350;
    TEST_mode.Forward_Speed         = 0;
    TEST_mode.Side_Speed            = 0;
    TEST_mode.PID_YAW_flag	  = 1;
    TEST_mode.PID_ROL_flag	  = 1;
    TEST_mode.PID_Depth_flag  = 1;


}

uint32_t motor_timer = 0;
void motor_task()
{
    //osDelay(3000);
    Motor_Init();
    Mode_Init();
    PidParameter_Init(&PID_ROL_Angle, &PID_YAW_Angle, &PID_Depth, &PID_PIT_Angle, &PID_View, &PID_ROL_Speed, &PID_PIT_Speed, &PID_YAW_Speed);
    //HAL_UART_Transmit_IT(&huart2, JY60_Mode, sizeof(JY60_Mode));
    //HAL_UART_Transmit_IT(&huart2, JY60_Calibrate, sizeof(JY60_Calibrate));



    //JY60_InitFlag = 0;
    osDelay(500);
    JY60_InitFlag = 0;
    Motor_Init();
    for (;;) {


        Motor_Pid(&TEST_mode);
        Motor_Cal(&TEST_mode);

        Set_Motor_Speed(Motor1);
        Set_Motor_Speed(Motor2);
        Set_Motor_Speed(Motor3);
        Set_Motor_Speed(Motor4);
        Set_Motor_Speed(Motor5);
        Set_Motor_Speed(Motor6);
        Set_Motor_Speed(Motor7);
        Set_Motor_Speed(Motor8);



        osDelay((1));
    }

}

